#include "a_star.h"

// 构造函数
AstarPlanner::AstarPlanner(){
    cout << "AstarPlanner initializtion......" << endl;
    __debug = false;
    for (int x = -1; x <= 1; x++)
        for (int y = -1; y <= 1; y++){
            if (x == 0 && y == 0) continue;
            std::shared_ptr<GP> new_gp = std::make_shared<GP>(x, y, hypot(x,y), 0, 0, nullptr);
            __neighbor_points.emplace_back(new_gp);
        }
}

// 设置调试模式
void AstarPlanner::SetDebugMode(bool debug){
    __debug = debug;
}

// 更新缓存数据
void AstarPlanner::UpdateCache(){
    
}

// 开始规划路径
bool AstarPlanner::GeneratePath(Pose2D start, Pose2D goal, vector<Pose2D>& points, vector<Pixel2D>& expend_points){
    // std::shared_ptr<GP> -> Pose2D
    auto GP2Pose2D = [&](std::shared_ptr<GP> gp) -> Pose2D
    {
        Pose2D pose_2d;
        pose_2d = map_2d.Pixel2DToPose2D(Pixel2D(gp->x, gp->y));
        return pose_2d;
    };

    // std::shared_ptr<GP> -> Pixel2D
    auto GP2Pixel2D = [&](std::shared_ptr<GP> gp) -> Pixel2D
    {
        return Pixel2D(gp->x, gp->y);
    };
    
    cout << "[AstarPlanner | GeneratePath] start......" << endl;
    points.clear();
    expend_points.clear();
    std::priority_queue<std::shared_ptr<GP>, std::vector<std::shared_ptr<GP>>, GP::compare_cost> open_list;
    std::unordered_map<int, std::shared_ptr<GP>> closed_list;
    vector<std::shared_ptr<GP>> points_gp, expend_points_gp;

    Pixel2D pixel_start = map_2d.Pose2DToPixel2D(start);
    Pixel2D pixel_goal = map_2d.Pose2DToPixel2D(goal);
    std::shared_ptr<GP> gp_start = std::make_shared<GP>(pixel_start.x, pixel_start.y, 0.0, 0.0, map_2d.ToIndex(pixel_start), nullptr);
    std::shared_ptr<GP> gp_goal = std::make_shared<GP>(pixel_goal.x, pixel_goal.y, 0.0, 0.0, map_2d.ToIndex(pixel_goal), nullptr);
    // gp_start.setIdx(map_2d.ToIndex(Pixel2D(gp_start->x, gp_start->y)));
    gp_start->setH(hypot(gp_goal->x - gp_start->x, gp_goal->y - gp_start->y) * 1.5);
    open_list.push(gp_start);
    cout << "[AstarPlanner | GeneratePath] gp_start x=" << gp_start->x << ", y=" << gp_start->y << ", idx=" << gp_start->idx
            << ", f=" << gp_start->f << ", g=" << gp_start->g << ", h=" << gp_start->h << endl;
            
    cout << "[AstarPlanner | GeneratePath] gp_goal x=" << gp_goal->x << ", y=" << gp_goal->y << ", idx=" << gp_goal->idx
            << ", f=" << gp_goal->f << ", g=" << gp_goal->g << ", h=" << gp_goal->h << endl;
            
    int debug_count = 0;
    int debug_count_max = __debug ? 100 : 0;
    while(!ros::isShuttingDown() && !open_list.empty()){
        // 获取优先级最高节点
        std::shared_ptr<GP> current = open_list.top();
        open_list.pop();

        if(closed_list.find(current->idx) != closed_list.end())
            continue; 
        closed_list.insert(std::make_pair(current->idx, current));

        expend_points_gp.push_back(current);

        if(__debug){
            debug_count ++;
            cout << "[" << debug_count << "] current x=" << current->x << ", y=" << current->y << ", idx=" << current->idx
                << ", f=" << current->f << ", g=" << current->g << ", h=" << current->h 
                << ", open_list.size=" << open_list.size()<< endl;
            if(debug_count > debug_count_max){
                cout << "[AstarPlanner::GetPath] open_list size: " << open_list.size() << endl;
                while(current->getParent() != nullptr){
                    cout << "[gp_goal] idx=" << current->idx
                        << ", parent_idx=" << (current->getParent() == nullptr ? "nullptr" : to_string(current->getParent()->idx))
                        << endl;
                    if(current->idx == current->getParent()->idx) break;
                    points_gp.emplace_back(current);
                    current = current->getParent();
                }
                for(auto gp : points_gp){
                    points.emplace_back(GP2Pose2D(gp));
                }
                for(auto gp : expend_points_gp){
                    expend_points.emplace_back(GP2Pixel2D(gp));
                }  
                cout << "[AstarPlanner::GetPath] points size: " << points.size() << endl;      
                return true;        
            }
        }

        // 找到终点，获取路径
        if(current->idx == gp_goal->idx){
            // path = _convertClosedListToPath(closed_list, start, goal);
            cout << "[AstarPlanner::GetPath] find goal. open_list size: " << open_list.size() << endl;
            while(current->getParent() != nullptr){
                points_gp.emplace_back(current);
                current = current->getParent();
            }
            std::reverse(points_gp.begin(), points_gp.end());

            for(auto gp : points_gp){
                points.emplace_back(GP2Pose2D(gp));
            }
            for(auto gp : expend_points_gp){
                expend_points.emplace_back(GP2Pixel2D(gp));
            }
            return true;
        }

        // 遍历邻近节点
        for(auto gp : __neighbor_points){
            std::shared_ptr<GP> new_gp = std::make_shared<GP>();
            new_gp->setX(current->x + gp->x);
            new_gp->setY(current->y + gp->y);
            new_gp->setG(current->g + gp->g);
            new_gp->setH(current->h + gp->h);
            new_gp->setIdx(map_2d.ToIndex(Pixel2D(new_gp->x, new_gp->y)));
            // if(__debug){
            //     cout << "--> [" << open_list.size() << "] new_gp x=" << new_gp->x << ", y=" << new_gp->y << ", idx=" << new_gp->idx
            //         << ", f=" << new_gp->f << ", g=" << new_gp->g << ", h=" << new_gp->h << endl;
            // }
            if (closed_list.find(new_gp->idx) != closed_list.end()){
                // if(__debug){
                //     cout << "--> idx: " << new_gp->idx << " in closed_list." << endl;
                // }
                continue;
            }
            if(!map_2d.IsValid(new_gp->idx)){
                // if(__debug){
                //     cout << "--> idx: " << new_gp->idx << " IsValid=false." << endl;
                // }
                continue;
            }
            if(!map_2d.ValueIsValid(Pixel2D(new_gp->x, new_gp->y))) continue;

            new_gp->setParent(current);
            new_gp->setH(hypot(gp_goal->x - new_gp->x, gp_goal->y - new_gp->y) * 1.5);
            if(__debug){
                cout << "--> [" << open_list.size() << "] new_gp x=" << new_gp->x << ", y=" << new_gp->y << ", idx=" << new_gp->idx
                    << ", parent_idx=" << (current->getParent() == nullptr ? "nullptr" : to_string(current->getParent()->idx))
                    << ", f=" << new_gp->f << ", g=" << new_gp->g << ", h=" << new_gp->h 
                    << endl;
            }
           
            open_list.push(new_gp);
        }

    }
    return false;
}